#include <ros/ros.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <linux/input.h>
#include <serial/serial.h>
#include <tf/tf.h>
#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int8.h>
#include <tecbot_msgs/battery_state.h>
#include <tecbot_msgs/emergency_request.h>
#include <tecbot_msgs/cmd_vel_mode_request.h>
#include <common/common_state_code.h>
#include <localization/localization_sources.h>

#define NODE_NAME "chassis_handler"
#define TAG "机器人底盘驱动"

using namespace std;

// 通讯定义
#define COMM_HEAD '#'
#define COMM_TAIL '!'
#define COMM_FILTER ' '
#define COMM_STATE_WAITFORHEAD 0
#define COMM_STATE_WAITFORTAIL 1
#define COMM_PACKET_BUFFER_SIZE 1024

// 定义参数
// 控制频率
float control_freq = 50;

// 连接参数
string port_name;
int baudrate;

// 电机参数
int wheel_encoder_pulses_per_round = 1024;
int wheel_max_speed_rpm = 130;

// 底盘参数
double chassis_radius = 0;
double wheel_radius = 0;
double odom_compensation_ratio = 1;

// 电池参数
double empty_voltage = 0;
double full_voltage = 0;

// 定义串口
serial::Serial serial_port;

// 定义通讯参数
uint8_t comm_state = COMM_STATE_WAITFORHEAD;
string packet_receive_buffer;
string packet_send_buffer;

// 定义电机数据结构
struct motor_speed_t
{
    double speed_left_pps;
    double speed_right_pps;
} motor_speed_data;

struct motor_encoder_t
{
    int32_t encoder_left;
    int32_t encoder_right;
};

struct motor_data_t
{
    motor_speed_t speed;
    motor_encoder_t encoder;
    uint16_t state;
} motor_data, last_motor_data;

// 定义航迹推算数据
struct odom_data_t
{
    double x;
    double y;
    double yaw;
    double vx;
    double vyaw;
} odom_data_raw;

nav_msgs::Odometry odom_data;

// 定义电池电量数据
struct battery_data_t
{
    double percent;
    double voltage;
} battery_data_raw;

tecbot_msgs::battery_state battery_data;

// 定义按钮数据
bool button_data_raw = false;

std_msgs::Bool button_data;

// 定义灯光数据
std_msgs::Int8 light_color_data;

// 定义速度控制量数据
geometry_msgs::Twist twist_data;

// 定义紧急停车标志
bool emergency_flag = false;

// 定义手动控制标志
bool manual_control_flag = true;

// 定义发布器
ros::Publisher odom_pub;
ros::Publisher battery_pub;
ros::Publisher button_pub;

// 定义接收器
ros::Subscriber cmd_vel_sub;
ros::Subscriber cmd_vel_manual_sub;
ros::Subscriber light_sub;

// 定义服务器
ros::ServiceServer emergency_server;
ros::ServiceServer manual_control_server;

// 功能函数
// Modbus 初始化函数
bool serial_init()
{
    bool ret = false;

    try
    {
        // 设置端口名称
        serial_port.setPort(port_name);

        // 设置端口波特率
        serial_port.setBaudrate(baudrate);

        // 设置端口超时
        serial::Timeout timeout = serial::Timeout::simpleTimeout(2000);
        serial_port.setTimeout(timeout);

        // 开启串口
        serial_port.open();

        ret = true;
    }
    catch (...)
    {
        ROS_ERROR("%s：机器人底盘串口连接失败。", TAG);
    }

    return ret;
}

// 更新电机数据
bool chassis_update_data(string packet, motor_data_t &motor_data, battery_data_t &battery_data, bool &button_data)
{
    bool ret = false;

    // 判断包
    int filter_counter = 0;

    for (int i = 0; i < packet.size(); i++)
    {
        if (packet[i] == COMM_FILTER)
        {
            filter_counter++;
        }
    }

    if (filter_counter == 5)
    {
        int last_pos = 0;
        int number_index = 0;
        int numbers[6] = {0};

        for (int i = 0; i < packet.size(); i++)
        {
            if (packet[i] == COMM_FILTER || i == packet.size() - 1)
            {
                numbers[number_index++] = stoi(packet.substr(last_pos, i - last_pos + 1));

                last_pos = i + 1;
            }
        }

        // 获取数据
        motor_data.speed.speed_left_pps = numbers[0] * 1.0;
        motor_data.speed.speed_right_pps = numbers[1] * 1.0;
        motor_data.encoder.encoder_left = numbers[2];
        motor_data.encoder.encoder_right = numbers[3];

        battery_data.voltage = numbers[4] / 10.0 + 1;
        battery_data.voltage = battery_data.voltage > 13 ? 13 : battery_data.voltage;
        double k = (100.0) / (full_voltage - empty_voltage);
        double b = -1 * (k * empty_voltage);
        battery_data.percent = k * battery_data.voltage + b;
        battery_data.percent = battery_data.percent < 0 ? 0 : (battery_data.percent > 100 ? 100 : battery_data.percent);

        button_data = (bool)numbers[5];

        ret = true;
    }

    return ret;
}

// 里程计解算
void odom_update_data(odom_data_t &data, const motor_data_t &motor_data, const motor_data_t &last_motor_data)
{
    // 计算机器人实际速度
    // 计算轮速
    double speed_left = motor_data.speed.speed_left_pps * 2 * M_PI * wheel_radius / wheel_encoder_pulses_per_round;
    double speed_right = motor_data.speed.speed_right_pps * 2 * M_PI * wheel_radius / wheel_encoder_pulses_per_round;

    // 计算线速度
    data.vx = odom_compensation_ratio * (speed_left + speed_right) / 2.0;

    // 计算角速度
    data.vyaw = odom_compensation_ratio * (speed_right - speed_left) / (2.0 * chassis_radius);

    // 计算机器人里程计
    // 计算编码脉冲差
    int delta_pulses_left = motor_data.encoder.encoder_left - last_motor_data.encoder.encoder_left;
    int delta_pulses_right = motor_data.encoder.encoder_right - last_motor_data.encoder.encoder_right;

    // 计算车轮行驶距离
    double delta_left = (2 * M_PI * wheel_radius * delta_pulses_left) / (1.0 * wheel_encoder_pulses_per_round);
    double delta_right = (2 * M_PI * wheel_radius * delta_pulses_right) / (1.0 * wheel_encoder_pulses_per_round);
    double delta_dist = (delta_left + delta_right) / 2.0;
    double delta_diff = (delta_right - delta_left) / 2.0;

    // 解算里程计
    data.x += odom_compensation_ratio * delta_dist * cos(data.yaw);
    data.y += odom_compensation_ratio * delta_dist * sin(data.yaw);
    data.yaw += odom_compensation_ratio * delta_diff / chassis_radius;
}

// 回调函数
// 速度控制回调
void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr &msg)
{
    if (!manual_control_flag)
    {
        // 解算速度
        if (emergency_flag)
        {
            motor_speed_data.speed_left_pps = 0;
            motor_speed_data.speed_right_pps = 0;
        }
        else
        {
            // 计算由角速度产生的线速度
            double line_speed = msg->angular.z * chassis_radius;

            // 计算车轮速度
            double speed_left = msg->linear.x - line_speed;
            double speed_right = msg->linear.x + line_speed;

            // 解算为电机 PPS
            motor_speed_data.speed_left_pps = speed_left * wheel_encoder_pulses_per_round / (2 * wheel_radius * M_PI);
            motor_speed_data.speed_right_pps = speed_right * wheel_encoder_pulses_per_round / (2 * wheel_radius * M_PI);

            // 防止电机速度溢出
            motor_speed_data.speed_left_pps =
                abs(motor_speed_data.speed_left_pps * 60.0 / wheel_encoder_pulses_per_round) > wheel_max_speed_rpm ? copysign(wheel_max_speed_rpm * wheel_encoder_pulses_per_round / 60.0, motor_speed_data.speed_left_pps) : motor_speed_data.speed_left_pps;
            motor_speed_data.speed_right_pps =
                abs(motor_speed_data.speed_right_pps * 60.0 / wheel_encoder_pulses_per_round) > wheel_max_speed_rpm ? copysign(wheel_max_speed_rpm * wheel_encoder_pulses_per_round / 60.0, motor_speed_data.speed_right_pps) : motor_speed_data.speed_right_pps;
        }
    }
}

// 手动速度控制回调
void cmd_vel_manual_callback(const geometry_msgs::Twist::ConstPtr &msg)
{
    if (manual_control_flag)
    {
        // 解算速度
        if (emergency_flag)
        {
            motor_speed_data.speed_left_pps = 0;
            motor_speed_data.speed_right_pps = 0;
        }
        else
        {
            // 计算由角速度产生的线速度
            double line_speed = msg->angular.z * chassis_radius;

            // 计算车轮速度
            double speed_left = msg->linear.x - line_speed;
            double speed_right = msg->linear.x + line_speed;

            // 解算为电机 PPS
            motor_speed_data.speed_left_pps = speed_left * wheel_encoder_pulses_per_round / (2 * wheel_radius * M_PI);
            motor_speed_data.speed_right_pps = speed_right * wheel_encoder_pulses_per_round / (2 * wheel_radius * M_PI);

            // 防止电机速度溢出
            motor_speed_data.speed_left_pps =
                abs(motor_speed_data.speed_left_pps * 60.0 / wheel_encoder_pulses_per_round) > wheel_max_speed_rpm ? copysign(wheel_max_speed_rpm * wheel_encoder_pulses_per_round / 60.0, motor_speed_data.speed_left_pps) : motor_speed_data.speed_left_pps;
            motor_speed_data.speed_right_pps =
                abs(motor_speed_data.speed_right_pps * 60.0 / wheel_encoder_pulses_per_round) > wheel_max_speed_rpm ? copysign(wheel_max_speed_rpm * wheel_encoder_pulses_per_round / 60.0, motor_speed_data.speed_right_pps) : motor_speed_data.speed_right_pps;
        }
    }
}

// 紧急制动回调
bool emergency_callback(tecbot_msgs::emergency_request::Request &req, tecbot_msgs::emergency_request::Response &resp)
{
    bool ret = false;

    emergency_flag = req.enable;

    resp.result = CMD_RESPONSE_OK;

    ret = true;

    return ret;
}

// 手动模式回调
bool cmd_vel_mode_callback(tecbot_msgs::cmd_vel_mode_request::Request &req, tecbot_msgs::cmd_vel_mode_request::Response &resp)
{
    bool ret = false;

    manual_control_flag = req.manual;

    resp.result = CMD_RESPONSE_OK;

    ret = true;

    return ret;
}

// 灯带颜色控制回调
void light_callback(const std_msgs::Int8::ConstPtr &msg)
{
    light_color_data.data = msg->data;
}

// 线程函数
void receive_and_parse_data_from_board()
{
    ros::Rate rate(200);

    while (ros::ok())
    {
        while (serial_port.available())
        {
            uint8_t current_char;

            // 读取一位
            serial_port.read(&current_char, 1);

            // 判断是否包头
            if (current_char == COMM_HEAD)
            {
                packet_receive_buffer = "";

                comm_state = COMM_STATE_WAITFORTAIL;
            }
            // 判断是否包尾
            else if (comm_state == COMM_STATE_WAITFORTAIL && current_char == COMM_TAIL)
            {
                // 更新电机数据
                bool result = chassis_update_data(packet_receive_buffer, motor_data, battery_data_raw, button_data_raw);

                if (result)
                {
                    // 更新里程计解算
                    odom_update_data(odom_data_raw, motor_data, last_motor_data);

                    // 发布里程计数据
                    odom_data.header.stamp = ros::Time::now();

                    odom_data.pose.pose.position.x = odom_data_raw.x;
                    odom_data.pose.pose.position.y = odom_data_raw.y;
                    odom_data.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odom_data_raw.yaw);

                    odom_data.twist.twist.linear.x = odom_data_raw.vx;
                    odom_data.twist.twist.angular.z = odom_data_raw.vyaw;

                    odom_pub.publish(odom_data);

                    last_motor_data = motor_data;

                    // 发布电池数据
                    battery_data.header.stamp = ros::Time::now();

                    battery_data.voltage = battery_data_raw.voltage;
                    battery_data.soc = battery_data_raw.percent;

                    battery_pub.publish(battery_data);

                    // 发布按钮数据
                    button_data.data = button_data_raw;

                    button_pub.publish(button_data);
                }

                packet_receive_buffer = "";

                comm_state = COMM_STATE_WAITFORHEAD;
            }
            // 否则加入队列
            else if (comm_state == COMM_STATE_WAITFORTAIL)
            {
                packet_receive_buffer += current_char;

                if (packet_receive_buffer.size() > COMM_PACKET_BUFFER_SIZE)
                {
                    comm_state = COMM_STATE_WAITFORHEAD;

                    packet_receive_buffer = "";
                }
            }
        }

        rate.sleep();
    }
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, NODE_NAME);

    // 设置控制台本地化，以显示中文
    setlocale(LC_CTYPE, "zh_CN.utf8");

    // 打印调试信息
    ROS_INFO("%s:节点已启动。", TAG);

    // 定义ROS节点处理程序
    ros::NodeHandle node_handle;

    // 定义ROS参数获取程序
    ros::NodeHandle node_handle_param("~/chassis");

    // 获取参数
    node_handle_param.param<float>("control_freq", control_freq, 50);

    node_handle_param.param<string>("port_name", port_name, "/dev/ttyUSB0");
    node_handle_param.param<int>("baudrate", baudrate, 115200);

    node_handle_param.param<int>("wheel_encoder_pulses_per_round", wheel_encoder_pulses_per_round, 4096);
    node_handle_param.param<int>("wheel_max_speed_rpm", wheel_max_speed_rpm, 100);

    node_handle_param.param<double>("chassis_radius", chassis_radius, 0.329);
    node_handle_param.param<double>("wheel_radius", wheel_radius, 0.085);
    node_handle_param.param<double>("odom_compensation_ratio", odom_compensation_ratio, 1.0);

    node_handle_param.param<double>("empty_voltage", empty_voltage, 10.0);
    node_handle_param.param<double>("full_voltage", full_voltage, 12.8);

    // 初始化系统
    if (serial_init())
    {
        // 初始化订阅器
        cmd_vel_sub = node_handle.subscribe<geometry_msgs::Twist>("tecbot/control/cmd_vel", 10, cmd_vel_callback);
        cmd_vel_manual_sub = node_handle.subscribe<geometry_msgs::Twist>("tecbot/control/cmd_vel_manual", 10, cmd_vel_manual_callback);
        light_sub = node_handle.subscribe<std_msgs::Int8>("tecbot/control/light", 10, light_callback);

        // 初始化发布器
        odom_pub = node_handle.advertise<nav_msgs::Odometry>("tecbot/hardware/odom", 1);
        battery_pub = node_handle.advertise<tecbot_msgs::battery_state>("tecbot/hardware/bms", 1, true);
        button_pub = node_handle.advertise<std_msgs::Bool>("tecbot/hardware/button", 1, true);

        // 初始化服务器
        emergency_server = node_handle.advertiseService("tecbot/control/emergency/request", emergency_callback);
        manual_control_server = node_handle.advertiseService("tecbot/control/cmd_vel_mode/request", cmd_vel_mode_callback);

        // 初始化数据
        odom_data.child_frame_id = LOCALIZATION_BASE_LINK;
        odom_data.header.frame_id = LOCALIZATION_ODOM;
        odom_data.pose.pose.orientation = tf::createQuaternionMsgFromYaw(0);

        // 启动读取线程
        boost::thread receive_thread(receive_and_parse_data_from_board);

        ROS_INFO("%s:读取线程已启动。", TAG);

        // 定义循环速度控制
        auto rate = ros::Rate(control_freq);

        while (node_handle.ok())
        {
            // 处理回调函数
            ros::spinOnce();

            // 发送数据
            packet_send_buffer = "";
            packet_send_buffer += COMM_HEAD;
            packet_send_buffer += to_string((int)(motor_speed_data.speed_left_pps));
            packet_send_buffer += COMM_FILTER;
            packet_send_buffer += to_string((int)(motor_speed_data.speed_right_pps));
            packet_send_buffer += COMM_FILTER;
            packet_send_buffer += to_string(light_color_data.data);
            packet_send_buffer += COMM_TAIL;

            try
            {
                serial_port.write(packet_send_buffer);
            }
            catch (...)
            {
                ROS_ERROR("%s：串口连接失败。", TAG);

                // 尝试重连
                serial_init();
            }

            // 循环频率控制
            rate.sleep();
        }
    }

    return 0;
}
